#include "LRCSearch.h"
#include "PID.h"
#include "common.h"
#include "interface.h"

extern PID DirCtrl;
static LRCSearchState LastState = LRC_BeforeStart;
static int LL, RR, CC = 0;
LRCSearchState LRCSearch(void) {
  LRCSearchState state = LRC_None;
  LL = SEARCH_L_IO == SEARCH_DETECT_COLOR ? 1 : 0;
  RR = SEARCH_R_IO == SEARCH_DETECT_COLOR ? 1 : 0;
  CC = SEARCH_M_IO == SEARCH_DETECT_COLOR ? 1 : 0;
  if (LL && RR) {
    // 左右同时检测到，不做处理
    return LastState;

  } else if (!LL && !CC && !RR) {
    if (LastState == LRC_BeforeStart)
      // 开车前别动
      return LRC_BeforeStart;
    else {
      // 直接返回丢线/无信息状态，不启动计时
      state = (LastState == LRC_L ? LRC_LnF
                                  : (LastState == LRC_R ? LRC_RnF : LRC_nF));
    }
  } else {
    state = LL | (CC << 1) | (RR << 2);
  }
  if (state != LastState) {
    LastState = state; // 检测到变化则刷新记录
  }
  return LastState;
}

void LRCReset(void) { LastState = LRC_None; }